#include <utils.hpp>

#if !defined(CONFIG_TAR4ESP32)
#include <sys/ipc.h>
#endif

#define MAX_SIZE_PATH (1024 * 1024)

// 获取程序所在目录
char* GetPrgPath(void)
{
	char* prg_path = (char*)malloc(MAX_SIZE_PATH);
	memset(prg_path, 0, MAX_SIZE_PATH);
	readlink("/proc/self/exe", prg_path, MAX_SIZE_PATH);
	char* p = nullptr;
	if (nullptr != (p = strrchr(prg_path, '/')))
	{
		*p = '\0';
	}
	else
	{
		return nullptr;
	}
	return prg_path;
}

// 获取程序所在目录下的文件名
char* GetPrgFilePath(const char* filename)
{
	char* prg_path = GetPrgPath();
	if (prg_path == nullptr)
	{
		return nullptr;
	}

	char* file_path = (char*)malloc(MAX_SIZE_PATH);
	memset(file_path, 0, MAX_SIZE_PATH);

	snprintf(file_path, MAX_SIZE_PATH, "%s/%s", prg_path, filename);

	free(prg_path);
	return file_path;
}


std::string StringTrimed(const std::string& str)
{
	size_t first = str.find_first_not_of(" \t\n\r\f\v"); // 查找第一个非空白字符
	if (std::string::npos == first)
	{
		return ""; // 如果没有找到非空白字符，返回原
	}

	size_t last = str.find_last_not_of(" \t\n\r\f\v"); // 查找最后一个非空白字符

	// 构造新的字符串，不包含首尾空白字符
	return str.substr(first, (last - first + 1));
}

// 将socket设置成非阻塞方式进行通信
int SocketNonBlocking(int socketFd)
{
	int flags  = 0;
	int status = 0;
	flags	   = fcntl(socketFd, F_GETFL, 0);
	if (flags == -1)
	{
		return -1;
	}
	flags |= O_NONBLOCK;
	status = fcntl(socketFd, F_SETFL, flags);
	if (status == -1)
	{
		return -1;
	}
	return 0;
}

// 校验socket是否存活，活1, 死0
int SocketCheckAlive(int socketFd)
{
#if !defined(CONFIG_TAR4ESP32)
	struct tcp_info info;
	int len = sizeof(info);
	getsockopt(socketFd, IPPROTO_TCP, TCP_INFO, &info, (socklen_t*)&len);
	if ((info.tcpi_state == TCP_ESTABLISHED))
	{
		return 1;
	}
	return 0;
#endif
	return 1;
}

// 得到当前时间
int64_t CurrentTimeUs(void)
{
	struct timeval tv_now;
	gettimeofday(&tv_now, nullptr);
	return (int64_t)tv_now.tv_sec * 1000000L + (int64_t)tv_now.tv_usec;
}

// 计算持续时间
double CalcDuarTime(int64_t& timeLast)
{
	// 得到当前时间
	int64_t time_us = CurrentTimeUs();
	// 计算持续时间
	double dt = (time_us - timeLast) / 1000000.0;
	// 更新上一次时间
	timeLast = time_us;
	// 返回持续时间
	return dt;
}

// 计算时间差
double CalcDiffTime(int64_t timeLast)
{
	// 得到当前时间
	int64_t time_us = CurrentTimeUs();
	// 计算持续时间
	double dt = (time_us - timeLast) / 1000000.0;
	// 返回持续时间
	return dt;
}

// 限幅
int LimitInt(int value, int min, int max)
{
	return std::min(std::max(value, min), max);
}

// 限幅
float LimitFloat(float value, float min, float max)
{
	return std::min(std::max(value, min), max);
}

// 限幅
double LimitDouble(double value, double min, double max)
{
	return std::min(std::max(value, min), max);
}

// 向量限幅
Eigen::Vector3d LimitVector3d(Eigen::Vector3d val, Eigen::Vector3d min, Eigen::Vector3d max)
{
	Eigen::Vector3d result;
	for (int i = 0; i < 3; i++)
	{
		result(i) = std::min(std::max(val(i), min(i)), max(i));
	}
	return result;
}

// 弧度转角度
double CoordArc2Angle(double arc)
{
	return arc * 180.0 / M_PI;
}

// 弧度转角度
Eigen::Vector3d CoordArc2Angle(Eigen::Vector3d arc)
{
	Eigen::Vector3d angle;
	for (int i = 0; i < 3; i++)
	{
		angle(i) = arc(i) * 180.0 / M_PI;
	}
	return angle;
}

// 角度转弧度
double CoordAngle2Arc(double angle)
{
	return angle * M_PI / 180.0;
}

// 角度转弧度
Eigen::Vector3d CoordAngle2Arc(Eigen::Vector3d angle)
{
	Eigen::Vector3d arc;
	for (int i = 0; i < 3; i++)
	{
		arc(i) = angle(i) * M_PI / 180.0;
	}
	return arc;
}

// 低通滤波
float FilterLowPass(float value, float& valPre, float filter)
{
	float result = value * filter + valPre * (1.0f - filter);
	valPre		 = result;
	return result;
}

// 低通滤波
double FilterLowPass(double value, double& valPre, double filter)
{
	float result = value * filter + valPre * (1.0 - filter);
	valPre		 = result;
	return result;
}

Eigen::Vector3d ConvertFromBodyToNED(Quaternion q, Eigen::Vector3d src)
{
	// 四元数乘法，将矢量旋转到机体坐标系
	double qw = q(0);
	double qx = q(1);
	double qy = q(2);
	double qz = q(3);

	double vx = src(0);
	double vy = src(1);
	double vz = src(2);

	Eigen::Vector3d result;
	// 四元数乘法公式
	result(0) = (1 - 2 * qy * qy - 2 * qz * qz) * vx + (2 * qx * qy - 2 * qz * qw) * vy + (2 * qx * qz + 2 * qy * qw) * vz;
	result(1) = (2 * qx * qy + 2 * qz * qw) * vx + (1 - 2 * qx * qx - 2 * qz * qz) * vy + (2 * qy * qz - 2 * qx * qw) * vz;
	result(2) = (2 * qx * qz - 2 * qy * qw) * vx + (2 * qy * qz + 2 * qx * qw) * vy + (1 - 2 * qx * qx - 2 * qy * qy) * vz;

	return result;
}